import matplotlib.pyplot as plt
import numpy as np
import math

T = 1
x_measure = [0] * 4
k = [1, 2, 3,4]
t = [0] * 4
K1 = [0] * 4
K2 = [0] * 4
RES = [0] * 4
x_prev = 0.0
xd_prev = 0.0
x_hat = [0] * 4
xd_hat = [0] * 4
xls_hat = [0] * 4
print('请输入：')
for i in range(0, 4):
    a = input()
    a = float(a)
    x_measure[i] = a
    t[i] = (k[i] - 1) * T
print('输入的是：',x_measure)

for i in range(0, 4):
    K1[i] = (2 *(2*k[i] - 1)) / (k[i] * (k[i] + 1))
    K2[i] = 6 / (k[i] * (k[i] + 1) * T )
    RES[i] = x_measure[i] - x_prev - xd_prev * T
    x_hat[i] = x_prev + xd_prev * T + K1[i] * RES[i]
    xd_hat[i] = xd_prev + K2[i] * RES[i]
    x_prev = x_hat[i]
    xd_prev = xd_hat[i]

for i in range(0, len(k)):
    t[i] =(k[i] - 1) * T

m1 = np.array([[4, sum(t)], [sum(t), sum(list(map(lambda x:x**2, t)))]])#lambda函数
w = [t * x_measure for t, x_measure in zip(t, x_measure)]#注意zip函数
m2 = np.array([[sum(x_measure)], [sum(w)]])

a = (np.linalg.inv(m1)). dot(m2)
a0 = float(a[0])
a1 = float(a[1])

for i in range(0, 4):
    t[i] = (k[i] - 1) * T
    xls_hat[i] = a0 + a1* t[i]


plt.ylabel('xhat')
plt.xlabel('Time(Sec)')
plt.plot(t, x_measure, '+')
plt.plot(t,xls_hat, '--',label = 'first-order least squares filters')
plt.plot(t, x_hat, '--',label = 'first-order rescurive least squares filters')
plt.legend()
plt.show()